/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file rssiTran.cxx
	\brief class implemenation for the RSSI Sensors

                    \author Maciej Stachura
             $Date: 2010/03/03 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "rssiTran.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
  /*<-End Global Variables-->*/

RSSITran::RSSITran(String name) : ThreadedPipe(name)
{
  buf_ptr = 0;
  new_meas = false;
  last_rssi_tx.stamp();
}

bool RSSITran::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	Client * me = shmPtr->clients.getClient(0);
  subscribe(CMD_GPS_LLA, me, true, INBOARD_TX);
	return true;
}

void RSSITran::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;
  const char * rssi_str;
  int rssi_meas = 0;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("RSSITran::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {
        int start_ptr = 0;
        if(buf_ptr == 0) 
          for(; start_ptr<n; start_ptr++)
            if (fifo_msg.data[start_ptr] == '\n')
              break;

        if(buf_ptr + n > RSSITRAN_BUF_SIZE)
          buf_ptr = 0;
        else {
          memcpy(&(buf[buf_ptr]), &(fifo_msg.data[start_ptr]), n-start_ptr);
          buf_ptr += n-start_ptr;

          int size = 0;
          for(; size < buf_ptr; size++){
            if((buf[size] == '\n') && (size > 1))
            {
              buf_ptr = 0;
              rssi_str = (const char *) buf;
              sscanf(rssi_str, "%i\n", &rssi_meas);
              new_meas = true;
              break;
            }
          }
        }
      }
    }
    if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs) && new_meas) {
      currentStamp.stamp();
      if(new_meas) {

        RSSIStruct_t * dataPtr = (RSSIStruct_t *)CMD_DATAPTR(fifo_msg.data);

        CMD_CMD((fifo_msg.data)) = CMD_RSSI_TELEM;
        CMD_NUM((fifo_msg.data)) = sizeof(RSSIStruct_t);
        dataPtr->rssi = rssi_meas;
        dataPtr->gps.lat = my_gps.lat;
        dataPtr->gps.lon = my_gps.lon;
        dataPtr->gps.alt = my_gps.alt;
        CMD_CSUM(fifo_msg.data) = 0;
        CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

        fifo_msg.size = CMD_SIZE(fifo_msg.data);

        Client * a_client;
        Client * me = shmPtr->clients.getClient(0);
        int i=0;
        while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_RSSI_TELEM,i)) != NULL)
        {
          fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
          fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
          writeFifo(INBOARD_TX);
          i++;
        }

        last_rssi_tx.stamp();
        new_meas = false;
      }
    }
    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(INBOARD_RX);
      if(n>0) {
        uint16_t length = CMD_SIZE(fifo_msg.data);
        if( length == fifo_msg.size) {

          Command cmd((unsigned char *)fifo_msg.data);
          if( !cmd.isValid() )
            cout << getName() << "::update() - error, invalid command" << endl << cmd ;
          else {
            parseInternalData(cmd);
					}
				}
      }
    }
  }
}

void RSSITran::parseInternalData(const Command &cmd)
{
 	Client * a_client = shmPtr->clients.getClient(fifo_msg.src);
	Client * me = shmPtr->clients.getClient(0);

	if(a_client == NULL) return;

	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

  switch(cmd.cmd() ) {
		case CMD_GPS_LLA: { // Got a new GPS packet
		  memcpy(&my_gps, dataPtr, sizeof(GPSLLA_t));
      break;
    }
  }
}
